/*
 * my_resampler.c
 *
 *  Created on: 19 août 2011
 *      Author: cedric
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "MyResampler.h"
#include "common.h"

//Struct holding member attributes
struct my_resampler {
	void (*callback)(sample_t* s, buffer_size_t size);
	uint32_t M; //window width
	uint32_t in_rate;
	uint32_t channels;
	uint32_t out_rate;
	uint32_t lcm;
	uint32_t in_taps;
	uint32_t out_taps;
	sample_t *chain; //delay line
	window_t *window; //window coeffs
	int32_t in_state;
	int32_t out_state;
	uint32_t out_counter;
	uint32_t out_delay;
	uint32_t in_delay;
	bool clip; // should we clip values above 1.0 (should be yes for PCM data). Defaults to true
	//buffer_size_t in_buf_size;
	buffer_size_t out_buf_size;
	sample_t* out_buffer;
	buffer_size_t out_buf_idx;
};

//public methods

MyResampler my_resampler_new(uint32_t ntaps, uint32_t input_rate,
		uint32_t out_rate, buffer_size_t output_buffer_size,
		my_resampler_window_function func,
		void(*callback)(sample_t* s, buffer_size_t size)) {

	//initialize object
	MyResampler rs = malloc(sizeof(struct my_resampler));
	rs->in_rate = input_rate;
	rs->out_rate = out_rate;
	rs->callback = callback;
	rs->out_counter = 0;
	rs->out_buf_size = output_buffer_size;
	rs->out_buf_idx = 0;
	rs->out_buffer = malloc(sizeof(sample_t) * rs->out_buf_size);
	rs->clip = true;

	//compute least common multiplicator between input and output samplerate
	rs->lcm = lcm(input_rate, out_rate);

	rs->M = ntaps;

	//number of intermediary frames per input frame
	rs->in_taps = rs->lcm / rs->in_rate;
	//number of intermediary frames per output frame
	rs->out_taps = rs->lcm / rs->out_rate;

	rs->in_state = rs->M;
	rs->out_state = rs->M;

	rs->out_delay = rs->M / rs->out_taps / 2;
	rs->in_delay = rs->M / rs->in_taps / 2;

	//TODO: M should be a multiple of the scm

	//the cutoff frequency for the lowpass filter is based on the lowest input/output samplerate
	uint32_t cutoff_freq = 2 * rs->lcm
			/ (input_rate <= out_rate ? input_rate : out_rate);

	puts("new version");
	printf(
			"Resampler initialized with input rate:%d Hz, output rate:%d Hz, window length:%d, lcm:%d, cutoff_freq:%d output_delay:%d\n",
			rs->in_rate, rs->out_rate, rs->M, rs->lcm, cutoff_freq,
			rs->out_delay);

	//init window values
	uint32_t i;

	//allocate and initialize M+1 values for the window
	//puts("window start:");
	rs->window = malloc((rs->M + 1) * 2 * sizeof(window_t));

	window_t (*win_func)(int32_t i, uint32_t cutoff_freq, uint32_t M);

	switch (func) {
	case WINDOW_NONE:
		win_func = &sinc_none;
		break;
	case WINDOW_BLACKMAN:
		win_func = &sinc_blackman;
		break;
	case WINDOW_KAISER:
		win_func = &sinc_none;
		break;
	default:
		win_func = &sinc_none;
		break;
	}

	for (i = 0; i <= rs->M; i++) {
		rs->window[i] = win_func(i - rs->M / 2, cutoff_freq, rs->M);
		//duplicate the value so that we have 2 windows (will be useful afterwards)
		rs->window[i + rs->M + 1] = rs->window[i];
		//printf("%.3f\n", rs->window[i]);
	}
	//puts("window end");

	//init chain of values to be processed
	rs->chain = calloc(rs->M, sizeof(sample_t));

	return rs;
}

void my_resampler_destroy(MyResampler rs) {
	sample_t* s = calloc(rs->in_delay, sizeof(sample_t));
	puts("added output");
	my_resampler_process_sample(rs, s, rs->in_delay);
	//write output buffer content
	if (rs->out_buf_idx > 0) {
		rs->callback(rs->out_buffer, rs->out_buf_idx);
	}
	free(s);
	free(rs->window);
	free(rs->chain);
	free(rs);
}

void my_resampler_set_clipping(MyResampler rs, bool clip) {
	rs->clip = clip;
}

int32_t circular_distance(int32_t first, int32_t next, uint32_t max_distance,
		uint32_t buffer_size) {
	int32_t result;
	if (abs(next - first) <= max_distance) {
		result = next - first;
	} else if (next < first) { //first is at the end of the buffer while next is at the beginning
		//TODO: make sure that buffer is wider than input_taps and output_taps
		result = next - first + buffer_size;
	} else { //next is at the end of the buffer while first is at the beginning
		result = next - first - buffer_size;
	}
	return result;
}

int32_t circular_shift(int32_t start, uint32_t gap, uint32_t steps,
		uint32_t buffer_size) {
	int32_t result = start + steps * gap;
	if (result >= buffer_size) {
		result -= buffer_size;
		if (result >= start) {
			return -1;
		}
	}
	return result;
}

void my_resampler_process_sample(MyResampler rs, sample_t* s,
		buffer_size_t size) {
	//printf("processing sample: %f\n", s);
	uint32_t w = rs->M + 1; //window size
	int32_t l = (w / rs->in_taps); //used afterwards
	buffer_size_t z;

	for (z = 0; z < size; z++) {

		//TODO use local variables

		//store input at the delay line current index
		//printf("storing value: %.3f\n", s);
		rs->chain[rs->in_state] = s[z];

		//store current index
		int32_t old_in_state = rs->in_state;

		//position to next index
		//manage circular buffer
		if ((rs->in_state -= rs->in_taps) < 0) {
			rs->in_state += w;
		}

		sample_t accum;

		uint32_t j;
		uint32_t win_offset;

		//process window with
		//	- chain = input data in circular buffer
		//	- window that is shifted by window_offset to match the chain position
		//we compute every output_taps samples until output_state goes beyond input_state
		while (circular_distance(rs->in_state, rs->out_state, rs->in_taps, w)
				> 0) {

			accum = 0;
			//compute FIR

			if ((win_offset = w - rs->out_state) > w) {
				win_offset = win_offset % w;
			}

			/*puts("processing chain/window values:");
			 for (j = 0; j <= rs->M; j++) {
			 if (j == rs->output_state)
			 printf("|");
			 printf("%.3f ", rs->chain[j]);
			 }
			 puts("");

			 for (j = 0; j <= rs->M; j++) {
			 if (window_offset == 0 || j + window_offset == w)
			 printf("|");
			 printf("%.3f ", rs->window[j + window_offset]);
			 }
			 puts("");
			 */

			//TODO use matrix
			//first index to multiply is based on input_state
			int32_t k = 0;
			j = old_in_state;
			bool end_reached = (j < rs->out_state) ? true : false; //no need to loop in this case

			while (k <= l && (end_reached == false || j < rs->out_state)) {
				accum += rs->chain[j] * rs->window[j + win_offset];
				/*printf("(%.3f)*(%.3f)\n", rs->chain[j],
				 rs->window[j + window_offset]);
				 */
				if ((j += rs->in_taps) >= w) {
					j -= w;
					end_reached = true;
				}
				k++;
			}

			//avoid issues with values above 1 or below -1
			if (rs->clip == true) {
				if (accum > 1.0) {
					accum = 1.0;
				} else if (accum < -1.0) {
					accum = -1.0;
				}
			}

			if (rs->out_counter > rs->out_delay) { //don't write first results

				if (rs->out_buf_idx >= rs->out_buf_size) {
					rs->callback(rs->out_buffer, rs->out_buf_size);
					rs->out_buf_idx = 0;
				}
				rs->out_buffer[rs->out_buf_idx] = accum;
				rs->out_buf_idx++;

			} else {
				//puts("dropped output");
				rs->out_counter++;
			}

			// shift output state & zero intermediary values & manage circular buffer
			if ((rs->out_state -= rs->out_taps) < 0) {
				rs->out_state += w;
			}
		}
	}
}

